#ifndef MECHANUM_H
#define MECHANUM_H

#ifdef __cplusplus
extern "C"
{
#endif
/*---------------------------- C Scope ---------------------------*/
/*电机位置标识*/
enum
{
  LF = 0,
  RF,
  RB,
  LB = 3
};

enum
{
  X = 0,
  Y,
  R = 2
};
#include "stdint.h"

#ifdef __cplusplus
}
#endif
/*---------------------------- C++ Scope ---------------------------*/
class Mechanum{
private:
    float x_target;
    float y_target;
    float r_target;
    float wheelTarget[4];
    float moveSpeed[3];

public:
    void Update(float _x_target,float _y_target,float _r_target);
    void MC_SpeedCalc(); //正运动学解算
    void MC_ReverseCalc(int16_t *_motor_speed);//逆运动学

    float* getWheelTarget(){return wheelTarget;}
    float* getMoveSpeed(){return moveSpeed;}
};


#endif
